#include "Pixie.h"
#include "Opal.h"
#include "message.h"
#include "printf.h"

generic module OpalSendStageP() {
  provides interface PixieStage;
  uses interface PixieSink;
  uses interface PixieMemAlloc;

} implementation {

  command error_t PixieStage.init() {
    return SUCCESS;
  }

  command error_t PixieStage.run(memref_t dequeuedMR) {
    memref_t newMR = PIXIE_NULL_MEMREF;
    OpalMsg* msgPtr;
    uint16_t *data;
    if (dequeuedMR == PIXIE_NULL_MEMREF) {
      return FAIL;
    } else {
        newMR = call PixieMemAlloc.allocate(sizeof(OpalMsg));
    	msgPtr = (OpalMsg*) call PixieMemAlloc.data(newMR);
        data = (uint16_t*) call PixieMemAlloc.data(dequeuedMR); 
	msgPtr->source = TOS_NODE_ID;
        msgPtr->data0 = *data;
        msgPtr->data1 = 0;
        msgPtr->data2 = 0;
        msgPtr->data3 = 0;
        printf("Sending packet, data=%d \n", *data);
        printfflush();
        call PixieSink.enqueue(newMR);
	call PixieMemAlloc.release(newMR);	
    }


    call PixieSink.enqueue(newMR); //pass the new memref downstream
    call PixieMemAlloc.release(newMR); //must release memref after done
    call PixieMemAlloc.release(dequeuedMR); //must release memref after done
    return SUCCESS;
  }
}
